#include "PoseGL.hpp"
#include "Utils.hpp"
#include <chrono>
#include <algorithm>
#include <iostream>
using namespace std::chrono_literals;

PoseGL* objectPtr;

PoseGL::PoseGL()
{
  objectPtr=this;
}

PoseGL::~PoseGL()
{
  UIthread.request_stop();
  UIthread.join();
}

void PoseGL::MultiThreadRun()
{
  initMut.lock();
  UIthread=std::jthread(&PoseGL::UIWork,this);
  initMut.lock();
}

void PoseGL::SignalThreadRun(std::function<bool(const std::array<Cood3D,25> &)> func)
{
  while(true)
  {
	  if (!glfwInit()) { exit(EXIT_FAILURE); }
	  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 4);
	  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 5);
	  GLFWwindow* window = glfwCreateWindow(600, 600, "Pose3D", NULL, NULL);
	  glfwMakeContextCurrent(window);
	  if (glewInit() != GLEW_OK) { exit(EXIT_FAILURE); }
	  glfwSwapInterval(1);

    glfwSetWindowSizeCallback(window,&PoseGL::window_size_callback);

	  Init(window);
    initMut.unlock();
    while(true)
    {
      if(func(poseList))
      {
        float x,y,z;
        for(auto & i : poseList)
        {
          x+=i.x;
          y+=i.y;
          z+=i.z;
        }
        objCenterX=x/poseList.size();
        objCenterY=y/poseList.size();
        objCenterZ=z/poseList.size();
        for(auto & i : poseList)
        {
          i.x-=objCenterX;
          i.y-=objCenterY;
          i.z-=objCenterZ;
        }
        bufferRefreshFlag=true;
      }
      
      if(glfwWindowShouldClose(window))
      {
        ExitFlag=true;
        break;
      }
      std::this_thread::sleep_for(10ms);
	  	display(window, glfwGetTime());
	  	glfwSwapBuffers(window);
	  	glfwPollEvents();
    }

	  glfwDestroyWindow(window);
	  glfwTerminate();
  }
}

void PoseGL::ShowPose(const std::array<Cood3D,25> & pose)
{ 
  std::lock_guard<std::mutex> lck(poseListMut);
  poseList=pose;
  float x,y,z;
  for(auto & i : pose)
  {
    x+=i.x;
    y+=i.y;
    z+=i.z;
  }
  objCenterX=x/pose.size();
  objCenterY=y/pose.size();
  objCenterZ=z/pose.size();
  for(auto & i : poseList)
  {
    i.x-=objCenterX;
    i.y-=objCenterY;
    i.z-=objCenterZ;
  }
  bufferRefreshFlag=true;
}

void PoseGL::UIWork(std::stop_token token)
{
	if (!glfwInit()) { exit(EXIT_FAILURE); }
	glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 4);
	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 5);
	GLFWwindow* window = glfwCreateWindow(600, 600, "Pose3D", NULL, NULL);
	glfwMakeContextCurrent(window);
	if (glewInit() != GLEW_OK) { exit(EXIT_FAILURE); }
	glfwSwapInterval(1);

  glfwSetWindowSizeCallback(window,&PoseGL::window_size_callback);

	Init(window);
  initMut.unlock();
  while(!token.stop_requested())
  {
    if(glfwWindowShouldClose(window))
    {
      ExitFlag=true;
      break;
    }
    std::this_thread::sleep_for(10ms);
		display(window, glfwGetTime());
		glfwSwapBuffers(window);
		glfwPollEvents();
  }

	glfwDestroyWindow(window);
	glfwTerminate();
}

void PoseGL::Init(GLFWwindow* window)
{
	renderingProgram1 = Utils::createShaderProgram("glsl/vertShader.glsl", "glsl/fragShader.glsl");
	renderingProgram2 = Utils::createShaderProgram("glsl/linevertShader.glsl", "glsl/linefragShader.glsl");

	glfwGetFramebufferSize(window, &width, &height);
	aspect = (float)width / (float)height;
	pMat = glm::perspective(1.0472f, aspect, 0.1f, 1000.0f);

	cameraX = 0.0f; cameraY = 0.0f; cameraZ = 2.0f;
  routVal=2.0f;
	setupVertices();
}

void PoseGL::display(GLFWwindow* window,double currentTime)
{
	glClear(GL_DEPTH_BUFFER_BIT);
	glClearColor(0.0, 0.0, 0.0, 1.0);
	glClear(GL_COLOR_BUFFER_BIT);

	glUseProgram(renderingProgram1);

	mvLoc = glGetUniformLocation(renderingProgram1, "mv_matrix");
	projLoc = glGetUniformLocation(renderingProgram1, "proj_matrix");

	vMat = glm::translate(glm::mat4(1.0f), glm::vec3(-cameraX, 0.0f , -cameraZ));

	glUniformMatrix4fv(projLoc, 1, GL_FALSE, glm::value_ptr(pMat));

  std::lock_guard<std::mutex> lck(poseListMut);
	for (displayLoopi = 0; displayLoopi<25; displayLoopi++) {
		tMat = glm::translate(glm::mat4(1.0f), 
                          glm::vec3(poseList[displayLoopi].x, 
                                    poseList[displayLoopi].y,
                                    poseList[displayLoopi].z));
    sMat=glm::scale(glm::mat4(1.0f),glm::vec3(0.02f,0.02f,0.02f));
    rMat=glm::rotate(glm::mat4(1.0f),routVal,glm::vec3(0.0f,1.0f,0.0f));
    xflipMat=glm::rotate(glm::mat4(1.0f),glm::pi<float>(),glm::vec3(1.0f,0.0f,0.0f));
		mMat = xflipMat * rMat * tMat * sMat;
		mvMat = vMat * mMat;

		glUniformMatrix4fv(mvLoc, 1, GL_FALSE, glm::value_ptr(mvMat));

		glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
		glVertexAttribPointer(0, 3, GL_FLOAT, false, 0, 0);
		glEnableVertexAttribArray(0);

		glDrawArrays(GL_TRIANGLES, 0, 36);
	}

  auto printMatrix=[](decltype(glm::mat4(1.0f)) val)
  {
    float* v=glm::value_ptr(val);
    std::cout<<">>>> v"<<std::endl;
    for(int i=0;i<4;i++)
    {
      for(int j=0;j<4;j++)
          std::cout<<v[i*4+j]<<' ';
      std::cout<<std::endl;
    }
  };

  //画线
	glUseProgram(renderingProgram2);
  if(bufferRefreshFlag)
  {
    setupLines();
    setupIndex();
    bufferRefreshFlag=false;
  }
  rMat=glm::rotate(glm::mat4(1.0f),routVal,glm::vec3(0.0f,1.0f,0.0f));
  xflipMat=glm::rotate(glm::mat4(1.0f),glm::pi<float>(),glm::vec3(1.0f,0.0f,0.0f));
	mvMat = vMat * xflipMat * rMat;
	mvLoc = glGetUniformLocation(renderingProgram2, "mv_matrix");
	projLoc = glGetUniformLocation(renderingProgram2, "proj_matrix");
	glUniformMatrix4fv(mvLoc, 1, GL_FALSE, glm::value_ptr(mvMat));
	glUniformMatrix4fv(projLoc, 1, GL_FALSE, glm::value_ptr(pMat));

  glBindBuffer(GL_ARRAY_BUFFER,vbo[1]);
  glVertexAttribPointer(1,3,GL_FLOAT,false,0,0);
  glEnableVertexAttribArray(1);
  glLineWidth(2);
  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,vbo[2]);
  // glDrawArrays(GL_LINE_STRIP,0,23);
  glDrawElements(GL_LINES,48,GL_UNSIGNED_INT,0);
}

void PoseGL::setupVertices()
{
	float vertexPositions[108] =
	{
    -1.0f,  1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, -1.0f, -1.0f,
		1.0f, -1.0f, -1.0f, 1.0f,  1.0f, -1.0f, -1.0f,  1.0f, -1.0f,
		1.0f, -1.0f, -1.0f, 1.0f, -1.0f,  1.0f, 1.0f,  1.0f, -1.0f,
		1.0f, -1.0f,  1.0f, 1.0f,  1.0f,  1.0f, 1.0f,  1.0f, -1.0f,
		1.0f, -1.0f,  1.0f, -1.0f, -1.0f,  1.0f, 1.0f,  1.0f,  1.0f,
		-1.0f, -1.0f,  1.0f, -1.0f,  1.0f,  1.0f, 1.0f,  1.0f,  1.0f,
		-1.0f, -1.0f,  1.0f, -1.0f, -1.0f, -1.0f, -1.0f,  1.0f,  1.0f,
		-1.0f, -1.0f, -1.0f, -1.0f,  1.0f, -1.0f, -1.0f,  1.0f,  1.0f,
		-1.0f, -1.0f,  1.0f,  1.0f, -1.0f,  1.0f,  1.0f, -1.0f, -1.0f,
		1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f,  1.0f,
		-1.0f,  1.0f, -1.0f, 1.0f,  1.0f, -1.0f, 1.0f,  1.0f,  1.0f,
		1.0f,  1.0f,  1.0f, -1.0f,  1.0f,  1.0f, -1.0f,  1.0f, -1.0f
	};

	glGenVertexArrays(1, vao);
	glBindVertexArray(vao[0]);
	glGenBuffers(3, vbo);

	glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
	glBufferData(GL_ARRAY_BUFFER, sizeof(vertexPositions), vertexPositions, GL_STATIC_DRAW);
}

void PoseGL::setupLines()
{
	glBindVertexArray(vao[0]);
  glBindBuffer(GL_ARRAY_BUFFER,vbo[1]);
  glBufferData(GL_ARRAY_BUFFER,sizeof(float)*3*25,poseList.data(),GL_DYNAMIC_DRAW);
}

void PoseGL::setupIndex()
{
	glBindVertexArray(vao[0]);
  glBindBuffer(GL_ARRAY_BUFFER,vbo[2]);
  glBufferData(GL_ARRAY_BUFFER,sizeof(LineIndex),LineIndex,GL_STATIC_DRAW);
}

void PoseGL::window_size_callback(GLFWwindow* win, int newWidth, int newHeight)
{
	objectPtr->aspect = (float)newWidth / (float)newHeight;
	glViewport(0, 0, newWidth, newHeight);
	objectPtr->pMat = glm::perspective(1.0472f, objectPtr->aspect, 0.1f, 1000.0f);
}

bool PoseGL::isExit()
{
  return ExitFlag;
}
